///////////////////////////////////////////////////////////
//
// File: HaptWiimote.h
//
// Description:
//
///////////////////////////////////////////////////////////

#ifndef _HAPT_WIIMOTE_H_
#define _HAPT_WIIMOTE_H_

// Local Includes
#include "HaptCtrlDecl.h"

// System Includes
#include <math.h>
#include <string>
#include <wtypes.h>    // Required for byte
#include <initguid.h>
#include <process.h>   // Required for handles

using namespace std;

// Forward Declarations
class HaptHIDDevice;

typedef unsigned int HaptWiimoteButton;
static const HaptWiimoteButton WIIMOTE_BUTTON_2     = 0;
static const HaptWiimoteButton WIIMOTE_BUTTON_1     = 1;
static const HaptWiimoteButton WIIMOTE_BUTTON_B     = 2;
static const HaptWiimoteButton WIIMOTE_BUTTON_A     = 3;
static const HaptWiimoteButton WIIMOTE_BUTTON_MIN   = 4;
static const HaptWiimoteButton WIIMOTE_BUTTON_HOME  = 5;
static const HaptWiimoteButton WIIMOTE_BUTTON_LEFT  = 6;
static const HaptWiimoteButton WIIMOTE_BUTTON_RIGHT = 7;
static const HaptWiimoteButton WIIMOTE_BUTTON_DOWN  = 8;
static const HaptWiimoteButton WIIMOTE_BUTTON_UP    = 9;
static const HaptWiimoteButton WIIMOTE_BUTTON_PLUS  = 10;

typedef char HaptWiiInput;
static const HaptWiiInput WII_INPUT_LEDS     = 0x11;
static const HaptWiiInput WII_INPUT_TYPE     = 0x12;
static const HaptWiiInput WII_INPUT_IR1      = 0x13;
static const HaptWiiInput WII_INPUT_STATUS   = 0x15;
static const HaptWiiInput WII_INPUT_WRITEMEM = 0x16;
static const HaptWiiInput WII_INPUT_READMEM  = 0x17;
static const HaptWiiInput WII_INPUT_IR2      = 0x1a;

typedef char HaptWiiOutput;
static const HaptWiiOutput WII_OUTPUT_STATUS    = 0x20;
static const HaptWiiOutput WII_OUTPUT_READDATA  = 0x21;
static const HaptWiiOutput WII_OUTPUT_BUTTON    = 0x30;
static const HaptWiiOutput WII_OUTPUT_BUTMOT	= 0x31;
static const HaptWiiOutput WII_OUTPUT_BUTMOTIR  = 0x33;
static const HaptWiiOutput WII_OUTPUT_EXTMOT    = 0x35;
static const HaptWiiOutput WII_OUTPUT_EXTMOTIR  = 0x37;

struct HaptWiiEvent 
{
    int type;
};

// TOXIC: Rework class
class HAPTCTRL_EXPORT HaptWiimote
{
  public:
    // Constructor / Destructor
	HaptWiimote();
    ~HaptWiimote();

    ////////////////////
    // State Methods
    /////
	
    // Query State
    bool isButtonPressed( HaptWiimoteButton eBut );
    bool isIR1() { return _abIR[0]; }
    bool isIR2() { return _abIR[1]; }
    bool isIR3() { return _abIR[2]; }
    bool isIR4() { return _abIR[3]; }

    bool getRawMotionData( unsigned int &x, unsigned int &y, unsigned int &z );
    bool getRawIRData( float &x1, float &y1, 
                       float &x2, float &y2,
                       float &x3, float &y3, 
                       float &x4, float &y4 );

    bool getMotionData( float &x, float &y, float &z );
    bool getIRData( float &x1, float &y1, 
                    float &x2, float &y2,
                    float &x3, float &y3, 
                    float &x4, float &y4 );
    bool getPosition( float &x, float &y, float &z );

    bool getNormIRData( float &x, float &y );

    float pitch();
    float roll();

    float pitchRad();
    float rollRad();

    bool rumble() { return _bRumble; }
    
	// Set State
    bool setLEDs( bool led1, bool led2, bool led3, bool led4 );
	bool setRumble( bool on );
    bool setButton( int iBut, bool bEnable );
    bool setDefaultState();

    bool setMode( byte mode );

    ////////////////////
    // Utility methods
    /////

    // Set internal HID device
    bool setDevice ( HaptHIDDevice *pDev ) 
      { _pHIDDevice = pDev; return true; }
    HaptHIDDevice* device() { return _pHIDDevice; }

    // Threaded data collection
	bool enableThread( bool bEnable );
	bool isThreaded() const { return _hThread != INVALID_HANDLE_VALUE; }

    // Set Call back method
    typedef bool (*WiiCallBack)( HaptWiimote* pMote, const HaptWiiEvent &wii );
    bool setCallback( WiiCallBack pCB ) 
      { _pCallback = pCB; return true; }

  protected:
    ////////////////////
    // Internal methods
    /////

    bool _handleEvents( byte* pByte );

    // Decode the data returned from the Wiimote
    bool _decodeData( byte *pByte );
    bool _decodeButtons( byte *pByte );
    bool _decodeMotion( byte *pByte );
    bool _decodeIR( byte *pByte );
    bool _decodeExtension( byte *pByte );
    bool _decodeStatus( byte *pByte );

    // Read / Write data block
    bool _writeData( int address, byte *pBuff, int size );
    bool _readData(  int address, int size, byte *pBuff );
    bool _encodeAddress( int address, byte *pByte );

    // Data return configuration
    void _requestType( byte type );
    void _requestIR( byte type );

    void _requestCalibration();

    /////////////////////
    // Utilitiy Methods
    bool _normalizeIR();

  public:
    /////////////////////
    // Postion Tracking Interface
    
    float _length( float x1, float y1, float x2, float y2 ) 
      { return sqrt( ((x2-x1)*(x2-x1))+((y2-y1)*(y2-y1)) ); }
    float _length( float *p1, float *p2 ) 
      { return sqrt( ((p2[0]-p1[0])*(p2[0]-p1[0]))+((p2[1]-p1[1])*(p2[1]-p1[1])) ); }
    float _length( unsigned int *p1, unsigned int *p2 ) 
      { return sqrt( ((p2[0]-p1[0])*(p2[0]-p1[0]))+((p2[1]-p1[1])*(p2[1]-p1[1]))*1.0f ); }
    float _distance( float len )
      { return 13538.0f*pow(len,-1.0072f); }
    inline bool _sort( int *p );
  
  protected:
    static int _swap2Bytes( const unsigned char* p );
    static int _swap4Bytes( const unsigned char* p );

  protected:
    ////////////////////
    // Member variables
    /////

    // Wiimote Cached State
	bool            _abLeds[4];
    bool            _abButtonPressed[11];
	bool			_bRumble;
    bool            _abIR[4];
    unsigned int    _asIR[4];
   
  public:
    float	        _fMotion[3];
    float           _fIR[4][2];
    
    float           _fNormIR[2];

  public:
    unsigned int	_uiRawMotion[3];
    unsigned int    _uiRawIR[4][2];

  protected:
    bool            _bCal;
    unsigned int    _uiMotionCal[3];
    unsigned int    _uiGCal[3];

    byte            _mode;
    bool            _bCont;

    // Thread Identifiers
    bool			_bThread;
	unsigned int    _threadID;
	HANDLE          _hThread;

    HaptHIDDevice  *_pHIDDevice;

    WiiCallBack     _pCallback;

    // 
    byte            _inBuf[256];
    byte            _outBuf[256];
    byte            _tmpBuf[256];

  public:
    ////////////////////
    // Static interface methods
    /////

    // Connect/Disconnect to Wiimotes
    static bool connect();
    static bool disconnect();

    // Access Wiimotes
    static int        numWiimotes();
    static HaptWiimote* wiimote( int idx );

    // Static callback method
	static unsigned CALLBACK HaptWiimote::wiimoteThreadCB(void * param);
};

bool 
HaptWiimote::_sort( int *p ) 
{
    int t, s, pos;
    
    // Sort Along the x-axis
    for ( pos=0; pos<4; pos++ ) {
        s=0;
        if ( _fIR[p[0]][0] > _fIR[p[1]][0]  ) {
            t = p[0];
            p[0] = p[1];
            p[1] = t;
            s++;
        }
        if ( _fIR[p[1]][0] > _fIR[p[2]][0]  ) {
            t = p[1];
            p[1] = p[2];
            p[2] = t;
            s++;
        }
        if ( _fIR[p[2]][0] > _fIR[p[3]][0]  ) {
            t = p[2];
            p[2] = p[3];
            p[3] = t;
            s++;
        }
        if ( s==0 ) break;
    }

    if ( _fIR[p[0]][1] > _fIR[p[1]][1]  ) {
        t = p[0];
        p[0] = p[1];
        p[1] = t;
    }

    if ( _fIR[p[2]][1] > _fIR[p[3]][1]  ) {
        t = p[2];
        p[2] = p[3];
        p[3] = t;
    }

    return true;
}

#endif // _HAPT_WIIMOTE_H_